//
// Created by lizhehao on 2/19/21.
//

#include "Camera.h"
#include "cameraManagement.h"
#include "constValues.h"
#include "ArmorBoxDetector/findArmorBoxes.h"

using namespace robowalker;

const vector<Point3f> POINT_3D_OF_ARMOR_BIG = vector<Point3f>
        { //metric is mm
                Point3f(-105, -30, 0), //top left
                Point3f(105, -30, 0),  //top right
                Point3f(105, 30, 0),   // bottom right
                Point3f(-105, 30, 0)  // bottom left
        };

const vector<Point3f>  POINT_3D_OF_ARMOR_SMALL = vector<Point3f>
        {
                Point3f(-65, -35, 0),	//tl
                Point3f(65, -35, 0),	//tr
                Point3f(65, 35, 0),		//br
                Point3f(-65, 35, 0)		//bl
        };


Camera::Camera(const string& paramFilePath, const double expectedFrameRate, const int exposureTime, const int imgHeight, const int imgWidth) {
    this->nBuffSize = 0;
    this->pFrameBuf = nullptr;
    this->params = CameraParams();
    this->is_opened = true;

    // 打开相机
    this->cameraHandle = openCamera(pFrameBuf, nBuffSize, stInfo, expectedFrameRate, exposureTime, imgHeight, imgWidth);
    if(nullptr == cameraHandle) {
        printf("[Error] camera handle is nullptr. \n");
        this->is_opened = false;
        return;
    }

    //读取相机参数
    FileStorage fs(paramFilePath, FileStorage::READ);
    if(fs.isOpened()) {
        fs["camera_matrix"] >> params.cameraMatrix;
        fs["distortion_coefficients"] >> params.distCoeffs;
    } else{
        printf("[Error] Cannot read camera params. \n");
        this->is_opened = false;
        return;
    }
}

Camera::~Camera() {
    cameraExit(this->cameraHandle);
    printf("Camera exit. \n");
}

bool Camera::is_open() const {
    return this->is_opened;
}

void* Camera::getCameraHandle() const {
    return this->cameraHandle;
}


cv::Mat * Camera::readOneFrame(Mat &output, int64_t &TimeStamp) {
    if(this->is_opened == false)
    {
        printf("[Error] readOneFrame: camera is not open!\n");
        return nullptr;
    }
    double t1 = getTickCount();
    //auto nRet = MV_CC_GetImageForBGR(cameraHandle, pFrameBuf, nBuffSize, &stInfo, 1000);
    auto nRet = MV_CC_GetOneFrameTimeout(cameraHandle, pFrameBuf, nBuffSize, &stInfo, 1000);
    double t2 = getTickCount();
    auto dt = (t2 - t1) / getTickFrequency() * 1000;
    //printf("MV_CC_GetImageForBGR time: %lf\`n", dt);
    //printf("MV_CC_GetOneFrame time: %lf\n", dt);

    if( nRet != 0 )
    {
        cout << "[Error]: GetImageForRGB:" << setbase(16) << nRet << endl;
        return nullptr;
    } else {
        auto width_fig = stInfo.nWidth;
        auto height_fig = stInfo.nHeight;
         Mat pImg;
        if (stInfo.enPixelType == PixelType_Gvsp_BGR8_Packed) {
            pImg = Mat(height_fig, width_fig, CV_8UC3, pFrameBuf); 
        }
        if (stInfo.enPixelType == PixelType_Gvsp_YUV422_YUYV_Packed) {
            pImg = Mat(height_fig, width_fig, CV_8UC2, pFrameBuf); 
        }
        if (stInfo.enPixelType == PixelType_Gvsp_BayerRG8) {
            pImg = Mat(height_fig, width_fig, CV_8UC1, pFrameBuf); 
        }
    // unsigned int        nFrameNum;          ///< \~chinese 帧号             \~english Frame Number
    // unsigned int        nDevTimeStampHigh;  ///< \~chinese 时间戳高32位\~english Timestamp high 32 bits
    // unsigned int        nDevTimeStampLow;   ///< \~chinese 时间戳低32位\~english Timestamp low 32 bits
    // unsigned int        nReserved0;         ///< \~chinese 保留，8字节对齐\~english Reserved, 8-byte aligned
    // int64_t             nHostTimeStamp;     ///< \~chinese 主机生成的时间戳\~english Host-generated timestamp

        // printf("Frame Info: nFrameNum [%u], nDevTimeStampHigh: [%u], nDevTimeStampLow: [%u], nHostTimeStamp:[%ld] \n", 
        //                stInfo.nFrameNum, stInfo.nDevTimeStampHigh, stInfo.nDevTimeStampLow, stInfo.nHostTimeStamp);
        output = pImg;
        TimeStamp = stInfo.nHostTimeStamp;
        return &output;
    }
}

Point2f Camera::getPixelSize(float angleX, float angleY) const
{
    auto cam = params.cameraMatrix;
    double fx=cam.at<double>(0,0);
    double fy=cam.at<double>(1,1);
    double cx=cam.at<double>(0,2);
    double cy=cam.at<double>(1,2);

    float px = tan(angleX / 180.0 * CV_PI) * fx ;
    float py = tan(angleY / 180.0 * CV_PI) * fy ;

    return Point2f{px,py};
}

pair<double, double> Camera::getAngle(const Point2f center) const {
    auto x = center.x;
    auto y = center.y;

    auto cam = params.cameraMatrix;
    auto dis = params.distCoeffs;
    double fx=cam.at<double>(0,0);
    double fy=cam.at<double>(1,1);
    double cx=cam.at<double>(0,2);
    double cy=cam.at<double>(1,2);
    double k1=dis.at<double>(0);
    double k2=dis.at<double>(1);
    double p1=dis.at<double>(2);
    double p2=dis.at<double>(3);

    Point2f pnt;
    vector<cv::Point2f>in;
    vector<cv::Point2f>out;
    in.push_back(Point2f(x,y));

    //调用opencv内置函数对像素点去畸变
    undistortPoints(in,out,cam,dis,noArray(),cam);

    pnt=out.front();
    double tanx=(pnt.x-cx)/fx;
    double tany=(pnt.y-cy)/fy;

    auto angleX = atan(tanx)/CV_PI*180.0;
    auto angleY = atan(tany)/CV_PI*180.0;

    return {angleX, angleY};
}

double Camera::getDistance(const int armorBoxType, const Point2f* vertices) const {
    vector<Point2f> detectedArmorPoints({vertices[0], vertices[1], vertices[2], vertices[3]});
    Mat rotationVec = Mat::zeros(3, 1, CV_64FC1);
    Mat translationVec = Mat::zeros(3, 1, CV_64FC1);
    if(armorBoxType == ARMORBOX_BIG)
    {
        solvePnP(POINT_3D_OF_ARMOR_BIG, detectedArmorPoints, params.cameraMatrix, params.distCoeffs, rotationVec, translationVec, false, SOLVEPNP_ITERATIVE);
    }
    else if(armorBoxType == ARMORBOX_SMALL)
    {
        solvePnP(POINT_3D_OF_ARMOR_SMALL, detectedArmorPoints, params.cameraMatrix, params.distCoeffs, rotationVec, translationVec, false, SOLVEPNP_ITERATIVE);
    }
    translationVec.at<double>(1, 0) -= DIST_BETWEEN_GUN_AND_CAMERA; // TODO
    double dist = sqrt(translationVec.dot(translationVec));

    return dist;
}

float Camera::getCurrentFrameRate() const {
    MVCC_FLOATVALUE* fv = (MVCC_FLOATVALUE*)malloc(sizeof(MVCC_FLOATVALUE));
    auto nRet = MV_CC_GetFrameRate(cameraHandle, fv);
    if(MV_OK != nRet)
        printf("[Error] Camera::getCurrentFrameRate(): MV_CC_GetFrameRate fail! nRet [%x]\n", nRet);
    else
        return fv->fCurValue;
}
